#!/usr/bin/env python3
# coding:utf-8

import time
import serial
import struct
from dataclasses import dataclass

# debug
import debugpy

class Imu_FS982AG:
    def __init__(self,config):
        self.config = config
        self.open_serial(self.config['devname'], self.config['bps'],2)
        
    def open_serial(self, devname, bps, timeout):
        self.sp = serial.Serial(devname, bps, timeout=timeout)
        if self.sp.isOpen():
            print("Serial open ",devname)
        else:
            print("Serial error",devname)

    def loop_run(self):
        data=[
0xaa ,0x55, 0x02 ,0x00 ,0x2c ,0x00 ,0xe1 ,0x5e ,0xb7 ,0x0b ,0x90 ,0x98 ,0x6d ,0x41 ,0xd4 ,0xb8,
0x4f ,0x3f ,0x11 ,0x9b ,0x43 ,0x3b ,0x68 ,0xc2 ,0x83 ,0x3e ,0xaf ,0x84 ,0x6d ,0xbc ,0x0a ,0xed,
0x78 ,0xbf ,0xa8 ,0xee ,0xdb ,0xbc ,0xf4 ,0xda ,0xc8 ,0x3c ,0x1a ,0x64 ,0x2f ,0x3c ,0x23 ,0xdc,
0xf6 ,0x41 ,0x8e ,0x77 ,0x48 ,0x3f
        ]
        timeout = 1/config['rate']
        while True:
            time.sleep(timeout)
            self.sp.write(data)


if __name__ == "__main__":

    config={
        'devname': "/dev/ttyImu",
        'bps': 115200,
        'rate': 1,
    }
    # debugpy.listen(6688)
    # debugpy.wait_for_client()
    # debugpy.breakpoint()
    tty = Imu_FS982AG(config)
    tty.loop_run()
        
